package path;

import geom.AxisAngle;
import geom.Geom;
import geom.PolarVector;
import geom.Vector;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Iterator;

import processing.core.*;

public class Path implements PConstants{
	ArrayList keyFrames;
	long startTime = 0;
	ArrayList drawers;
	PApplet pa;
	boolean ignoreEquals = false;
	boolean smoothen;
	float angleThreshold = PI/6f;
	float distThreshold = 6f;
	float minDistance;
	float maxAngle;
	boolean smooth;
	
	public Path(PApplet pa) {
		keyFrames = new ArrayList();
		drawers = new ArrayList();
		minDistance = 1f;
		smooth = false;
		this.pa = pa;
	}
	
	public Path(PApplet pa, float minDistance) {
		keyFrames = new ArrayList();
		drawers = new ArrayList();
		this.minDistance = minDistance;
		this.pa = pa;
	}
	
	public ArrayList<KeyFrame> getKeyFrames(){
		return keyFrames;
	}
	
	public void addFrame(KeyFrame kf){
		if(kf.getTime() - startTime < getLastFrame()){
			System.out.println("Warning: time of added frame is earlier than last keyframe");
			return;
		}
		if(keyFrames.size() <= 0) {
			if(smooth) addSmoothed(kf);
			else{
				keyFrames.add(kf);
				notifyAdded();
			}
			startTime = kf.getTime();
		}
		kf.setTime(kf.getTime() - startTime);
		if(keyFrames.size() > 0){
			KeyFrame prev = getKeyFrameByIndex(keyFrames.size() - 1);
			prev.setLength(kf.getTime() - prev.getTime());
			if(ignoreEquals || prev.getPosition().getDistance(kf.getPosition()) >= 5){
				if(smooth) addSmoothed(kf);
				else{
					keyFrames.add(kf);
					notifyAdded();
				}
			}
		}

	}
	
	
	

	
	public void addFrames(ArrayList l){
		Iterator i = l.iterator();
		while(i.hasNext()){
			addFrame((KeyFrame)i.next());
		}
	}
	
	public void removeOldest(){
		keyFrames = new ArrayList(keyFrames.subList(1, keyFrames.size()));
		notifyRemoved(0);
	}
	
	public void removeKeyFrame(int index){
		keyFrames.remove(index);
		Iterator i = drawers.iterator();
		//System.out.println("Path Remove" + index + " size " + getSize());
		while(i.hasNext()){
			((PathDrawer)i.next()).keyRemoved(this, index);
		}
	}
	
	public long getLastFrame(){
		if(keyFrames.size() <= 0) return 0;
		return getKeyFrameByIndex(keyFrames.size()-1).getTime();
	}
	
	public long getFirstFrame(){
		if(keyFrames.size() <= 0) return 0;
		return getKeyFrameByIndex(0).getTime();
	}
	
	public long getLength(){
		if(keyFrames.size() < 1) return 0;
		return getLastFrame() - getFirstFrame() + getKeyFrameByIndex(keyFrames.size() - 1).getLength();
	}
	
	public KeyFrame getKeyFrame(long f){
		return getKeyFrameByIndex(getFrameIndex(f));
	}
	
	public int getFrameIndex(long f){
		return Collections.binarySearch(keyFrames, new KeyFrame(null, null, f));
	}
	
	public int getFrameIndex(KeyFrame kf){
		return Collections.binarySearch(keyFrames, kf);
	}
	
	public KeyFrame getKeyFrameByIndex(int i){
		//if(i >= keyFrames.size()) return null;

		return (KeyFrame)keyFrames.get(i);
	}
	
	public void addDrawer(PathDrawer drawer){
		drawers.add(drawer);
	}
	
	public void clearDrawers(){
		drawers = new ArrayList();
	}
	
	public KeyFrame getLastKeyFrame(){
		return getKeyFrameByIndex(keyFrames.size() -1);
	}
	
	public int getSize(){
		return keyFrames.size();
	}
	
	public void orientToPath(PApplet pa, int frameIndex){
		
		Vector v0, v1, v2;
		if(keyFrames.size() < 2) {
			System.out.println("not enough points");
			return;
		}
		
		v1 = getKeyFrameByIndex(frameIndex).getPosition();
			
		v0 = v1;
		int min = 1;
		while(v1.equalTo(v0) && frameIndex - min >= 0){
			v1 = getKeyFrameByIndex(frameIndex - min).getPosition(); 
			min++;
		}
		
		v2 = v1;
		int max = 1;
		while(v2.equalTo(v0) && frameIndex + max < keyFrames.size()){
			v2 = getKeyFrameByIndex(frameIndex + max).getPosition(); 
			max++;
		}
		
		if(v1.equalTo(v0) && v1.equalTo(v2)) {
			System.out.println("all points equal");
			return;
		}
		
		Geom.orientToPath(pa, v0, v1, v2, new Vector());
	}
	
	public void draw(PApplet pa){
		Iterator i = drawers.iterator();
		while(i.hasNext())
			((PathDrawer)i.next()).drawPath(this, pa);
	}
	
	private void notifyAdded(){
		Iterator i = drawers.iterator();
		while(i.hasNext()){
			((PathDrawer)i.next()).keyAdded(this);
		}
	}
	
	private void notifyAdded(int amount){
		Iterator i = drawers.iterator();
		while(i.hasNext()){
			((PathDrawer)i.next()).keysAdded(this, amount);
		}
	}
	
	private void notifyRemoved(int index){
		Iterator i = drawers.iterator();
		while(i.hasNext()){
			((PathDrawer)i.next()).keyRemoved(this, index);
		}
	}
	
	private float ratio = 1;
	private float prevRatio = 1;
	
	public void addSmoothed(KeyFrame kf){
		
		kf.original = true;

		if(keyFrames.size() < 3){
			keyFrames.add(kf);
			notifyAdded();
			return;
		}
		
		ArrayList newFrames = new ArrayList();
		KeyFrame v1, v2, v3, v4;
		

		v1 = getOriginal(keyFrames, keyFrames.size()-1, -2);
		if(v1 == null) { v1 = getOriginal(keyFrames, keyFrames.size()-1, -1); }
		v2 = getOriginal(keyFrames, keyFrames.size()-1, -1);
		v3 = (KeyFrame)keyFrames.get(keyFrames.size()-1);
		v4 = kf;
		//System.out.println(v1.toString() + v2.toString() + v3.toString() + v4.toString());
		
		float angleDiff = getAngleDiffrence(v2.getPosition(), v3.getPosition(), v4.getPosition());
		if(angleDiff > angleThreshold){
			
			boolean v2Found = false;
			while(!v2Found){
				//System.out.println("size: " + keyFrames.size());
				if(v2.isEqualTo((KeyFrame)keyFrames.get(keyFrames.size()-1))){
					v2Found = true;
				}
				else{
					removeKeyFrame(keyFrames.size()-1);
				}
			}
			
			prevRatio = ratio;
			ratio = angleDiff / angleThreshold;;
			
			
			if(Geom.distance(v2.getPosition(), v3.getPosition()) > distThreshold){
				float[] points = getInterpolations(prevRatio, ratio);
				
				float kfLength = v2.getLength();
				if(points.length > 0) 
					v2.setLength(Math.round(kfLength * points[0]));
				
				for(int i = 0; i < points.length; i++){
					Vector vb = curveInterpolate(v1.getPosition(), v2.getPosition(), v3.getPosition(), v4.getPosition(), points[i]);
					KeyFrame kfNew = new KeyFrame(vb, v2.getTime() + Math.round(kfLength * points[i]));
					kfNew.original = false;
					if(i < points.length - 1)
						kfNew.setLength(Math.round(kfLength * (points[i+1] - points[i])));
					
					else
						kfNew.setLength(Math.round(kfLength * (1 - points[i])));
					
					newFrames.add(kfNew);
				}
			}
			
			newFrames.add(v3);
			
			
			if(Geom.distance(v3.getPosition(), v4.getPosition()) > distThreshold){
				float[] points = getInterpolations(ratio, 1);
				
				float kfLength = v3.getLength();
				if(points.length > 0) 
					v3.setLength(Math.round(kfLength * points[0]));
				
				for(int i = 0; i < points.length; i++){
					Vector vb = curveInterpolate(v2.getPosition(), v3.getPosition(), v4.getPosition(), v4.getPosition(), points[i]);
					KeyFrame kfNew = new KeyFrame(vb, v3.getTime() + Math.round(kfLength * points[i]));
					kfNew.original = false;
					if(i < points.length - 1)
						kfNew.setLength(Math.round(kfLength * (points[i+1] - points[i])));
					else
						kfNew.setLength(Math.round(kfLength * (1 - points[i])));
					
					newFrames.add(kfNew);
				}
			}
			
			newFrames.add(v4);
			
			keyFrames.addAll(newFrames);
			notifyAdded(newFrames.size());
		}
		else{
			keyFrames.add(kf);
			notifyAdded();
		}
	}
	
	public void addSmoothedOLD(KeyFrame kf){
		kf.original = true;
		keyFrames.add(kf);
		notifyAdded(keyFrames.size() - 1);

		if(keyFrames.size() < 3){
			return;
		}
		
		//System.out.println("size >= 3");
		int index = keyFrames.size()-2;
		
		boolean originalFound = false;
		while(!originalFound && index - 1 >= 0){
			KeyFrame t =(KeyFrame)keyFrames.get(index - 1);
			if(t.original)
				originalFound = true;
			else{
				keyFrames.remove(index-1);
				notifyRemoved(index-1);
				index--;
			}
		}

		KeyFrame v1, v2, v3, v4, v5;
		

		v1 = getOriginal(keyFrames, index, -2);
		if(v1 == null) {
			v1 = getOriginal(keyFrames, index, -1);
		}
		v2 = getOriginal(keyFrames, index, -1);
		
		v3 = (KeyFrame)keyFrames.get(index);
		v4 = getOriginal(keyFrames, index, 1);
		v5 = getOriginal(keyFrames, index, 2);
		if(v5 == null) {
			v5 = getOriginal(keyFrames, index, 1);
		}
		//System.out.println(v1.toString() + v2.toString() + v3.toString() + v4.toString() + v5.toString());
		float angleDiff = getAngleDiffrence(v2.getPosition(), v3.getPosition(), v4.getPosition());
		if(angleDiff > angleThreshold){
			
			
			prevRatio = ratio;
			ratio = angleDiff / angleThreshold;;

			float[] points = getInterpolations(prevRatio, ratio);
			
			float kfLength = v2.getLength();
			if(points.length > 0) 
				v2.setLength(Math.round(kfLength * points[0]));
			
			for(int i = 0; i < points.length; i++){
				Vector vb = curveInterpolate(v1.getPosition(), v2.getPosition(), v3.getPosition(), v4.getPosition(), points[i]);
				KeyFrame kfNew = new KeyFrame(vb, Math.round(v2.getTime() + kfLength * points[i]));
				kfNew.original = false;
				if(i < points.length - 1)
					kfNew.setLength(Math.round(kfLength * (points[i+1] - points[i])));
				
				else
					kfNew.setLength(Math.round(kfLength * (1 - points[i])));
				
				keyFrames.add(index + i, kfNew);
				notifyAdded(index + i);
			}
			
			points = getInterpolations(ratio, 1);
			
			kfLength = v3.getLength();
			if(points.length > 0) 
				v3.setLength(Math.round(kfLength * points[0]));
			for(int i = 0; i < points.length; i++){
				Vector vb = curveInterpolate(v2.getPosition(), v3.getPosition(), v4.getPosition(), v5.getPosition(), points[i]);
				KeyFrame kfNew = new KeyFrame(vb, Math.round(v3.getTime() + kfLength * points[i]));
				kfNew.original = false;
				if(i < points.length - 1)
					kfNew.setLength(Math.round(kfLength * (points[i+1] - points[i])));
				else
					kfNew.setLength(Math.round(kfLength * (1 - points[i])));
				
				keyFrames.add(keyFrames.size() - 1, kfNew);
				notifyAdded(keyFrames.size() - 1);
			}
		}
	}
	
	private KeyFrame getOriginal(ArrayList list, int i, int c){
		int count = 0;
		int step = 1;
		if(c < 0) step = -1;
		KeyFrame original = null;
		i += step;
		
		while(count != c && i >= 0 && i < list.size()){
			
			KeyFrame current = (KeyFrame)list.get(i);
			if(current.original){
				count += step;
				original = current;
			}
			i += step;
		}
		if(original == null) System.out.println("no original found: index: " + i + " c: " + c);
		return original;
	}
	
	private float[] getInterpolations(float l1, float l2){
		int min = 1;
		int max = 7;
		
		int fl1 = (int)Math.floor(l1);
		int fl2 = (int)Math.floor(l2);

		if((fl1 + fl2) % 2 != 0){
			float d1 = l1 - fl1 - 0.5f;
			float d2 = l2 - fl2 - 0.5f;
			if(fl1 >= max && fl2 < max)
				fl1--;
			else if(fl2 >= max && fl1 < max)
				fl2--;
			else if(fl1 <= min && fl2 > min)
				fl1++;
			else if(fl2 <= min && fl1 > min)
				fl2++;
			else if(d1 >= d2)
				fl1++;
			else 
				fl2++;

		}

		int avg = (fl1 + fl2) / 2;
		if(avg <= 0) System.out.println("l1 " + l1 + " fl1 " + fl1 + " l2 " + l2 + " fl2 " + fl2);
		float[] ret = new float[avg];
		
		float s1 = 1f/fl1;
		float s2 = 1f/fl2;
		float total = 0;
		
		for(int i = 1; i <= avg; i++){
			ret[i-1] = (avg - i) * s1 / avg + i * s2 / avg; 
			if(i > 1)
				ret[i-1] +=ret[i-2];
		}	
		
		total = ret[avg-1];

		for(int i = 1; i <= avg; i++)
			ret[i-1] /= total;

		float[] r = new float[ret.length-1];
		for(int i = 0; i < r.length; i++){
			r[i] = ret[i];
		}
		return r;
	}
	
	private float getAngleDiffrence(Vector v1, Vector v2, Vector v3){
		AxisAngle aa1 = Geom.eulerToAxisAngle(v1.sub(v2));
		AxisAngle aa2 = Geom.eulerToAxisAngle(v2.sub(v3));
		return Math.abs(aa1.subtract(aa2).getA());
	}
	
	private Vector curveInterpolate(Vector v1, Vector v2, Vector v3, Vector v4, float t){
		float x = pa.curvePoint(v1.x, v2.x, v3.x, v4.x, t);
		float y = pa.curvePoint(v1.y, v2.y, v3.y, v4.y, t);
		float z = pa.curvePoint(v1.z, v2.z, v3.z, v4.z, t);
		return new Vector(x, y, z);
	}
	
	public String toString(){
		Iterator i = keyFrames.iterator();
		StringBuffer sb = new StringBuffer();
		while(i.hasNext()){
			sb.append(((KeyFrame)i.next()).toString());
			sb.append("\n");
		}
		return sb.toString();
	}
}
